#include <cstdio>
#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/GetMap.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "geometry_msgs/Quaternion.h"

using namespace std;

class MapGenerator
{
  public:
    MapGenerator(const std::string& mapname, int threshold_occupied, int threshold_free)
      : mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)
    {
      ros::NodeHandle n;
      ROS_INFO("Waiting for the map");
      map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this);
    }

    void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
    {
      ROS_INFO("Received a %d X %d map @ %.3f m/pix",
               map->info.width,
               map->info.height,
               map->info.resolution);

      width = map->info.width;
      height = map->info.height;
      resolution = map->info.resolution;
      originX = map->info.origin.position.x;
      originY = map->info.origin.position.y;

      std::string mapdatafile = mapname_ + ".pgm";
      std::string mappngfile = mapname_ + ".png";
      ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
      FILE* out = fopen(mapdatafile.c_str(), "w");
      if (!out)
      {
        ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
        return;
      }

      fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
              map->info.resolution, map->info.width, map->info.height);
      for(unsigned int y = 0; y < map->info.height; y++) {
        for(unsigned int x = 0; x < map->info.width; x++) {
          unsigned int i = x + (map->info.height - y - 1) * map->info.width;
          if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
            fputc(254, out);
          } else if (map->data[i] >= threshold_occupied_) { // (occ,255]
            fputc(000, out);
          } else { //occ [0.25,0.65]
            fputc(205, out);
          }
        }
      }

      fclose(out);

      //pgm->png
      Mat in_img;
      in_img = cv::imread(mapdatafile,0);
      if(in_img.empty()){

        cout << "err!!! input image cannot be read..\n";
        return ;
      }
      ROS_INFO("pgm->png");
      imwrite(mappngfile,in_img);

      std::string mapmetadatafile = mapname_ + ".yaml";
      ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
      FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
      /*
resolution: 0.100000
origin: [0.000000, 0.000000, 0.000000]
#
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

       */

      geometry_msgs::Quaternion orientation = map->info.origin.orientation;
      tf2::Matrix3x3 mat(tf2::Quaternion(
        orientation.x,
        orientation.y,
        orientation.z,
        orientation.w
      ));
      double yaw, pitch, roll;
      mat.getEulerYPR(yaw, pitch, roll);

      fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
              mappngfile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);

      fclose(yaml);

      ROS_INFO("Done\n");
      saved_map_ = true;
    }

    std::string mapname_;
    ros::Subscriber map_sub_;
    bool saved_map_;
    int threshold_occupied_;
    int threshold_free_;

      int height;
      int width;
      double originX;
      double originY;
      double resolution;

};


class MapSaver{
public:
  MapSaver(const std::string& mapname,const std::string& mappngname)
      : mapname_(mapname),png_path(mappngname) ,saved_map_(false)
    {
      ros::NodeHandle n;
      ROS_INFO("Waiting for the map");
      map_sub_ = n.subscribe("map", 1, &MapSaver::mapCallback, this);
    }
  void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
  {
      ROS_INFO("Received a %d X %d map @ %.3f m/pix",
              map->info.width,
              map->info.height,
              map->info.resolution);
      width = map->info.width;
      height = map->info.height;
      resolution = map->info.resolution;
      originX = map->info.origin.position.x;
      originY = map->info.origin.position.y;
      ROS_INFO("Writing map occupancy data to %s", mapname_.c_str());
      FILE* out = fopen(mapname_.c_str(), "w");
      if (!out)
      {
      ROS_ERROR("Couldn't save map file to %s", mapname_.c_str());
      return;
      }

      fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
              map->info.resolution, map->info.width, map->info.height);
      for(unsigned int y = 0; y < map->info.height; y++) {
      for(unsigned int x = 0; x < map->info.width; x++) {
          unsigned int i = x + (map->info.height - y - 1) * map->info.width;
          if (map->data[i] >= 0 && map->data[i] <= 25) { // [0,free)
          fputc(254, out);
          } else if (map->data[i] >= 65) { // (occ,255]
          fputc(000, out);
          } else { //occ [0.25,0.65]
          fputc(205, out);
          }
      }
      }

      fclose(out);
      Mat in_img;
      in_img = cv::imread(mapname_,0);
      if(in_img.empty()){

          cout << "err!!! input image cannot be read..\n";
          return;
      }
      ROS_INFO("pgm->png");
      imwrite(png_path,in_img);

      ROS_INFO("Done\n");
      saved_map_ = true;
  }

	std::string mapname_;
  std::string png_path;

	ros::Subscriber map_sub_;
	bool saved_map_;

  int height;
  int width;
  double originX;
  double originY;
  double resolution;
};